﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace InverseKinematics
{
    /// <summary>
    /// Represents a chain link of the class BoneChain
    /// </summary>
    public class Bone
    {
        /// <summary>
        /// the bone's appearance
        /// </summary>
        private Cuboid cuboid;

        /// <summary>
        /// the bone's last calculated angle if errors occure like not a number
        /// this will be used instead
        /// </summary>
        public float lastAngle = 0;

        private Vector3 worldCoordinate, destination;
        
        /// <summary>
        /// where the bone does point at
        /// </summary>
        public Vector3 Destination
        {
            get { return destination; }
            set { destination = value; }
        }

        /// <summary>
        /// the bone's source position
        /// </summary>
        public Vector3 WorldCoordinate
        {
            get { return worldCoordinate; }
            set { worldCoordinate = value; }
        }

        /// <summary>
        /// Generates a bone by another bone's end
        /// </summary>
        /// <param name="lastBone">the bone's end for this bone's source</param>
        /// <param name="destination"></param>
        public Bone(Bone lastBone, Vector3 destination) : this(lastBone.Effector, destination)
        {
        }

        /// <summary>
        /// Generates a bone at a coordinate in 
        /// </summary>
        /// <param name="worldCoordinate"></param>
        /// <param name="destination"></param>
        public Bone(Vector3 worldCoordinate, Vector3 destination)
        {
            cuboid = new Cuboid();
            this.worldCoordinate = worldCoordinate;
            this.destination = destination;
        }

        /// <summary>
        /// calculate's the bone's appearance appropiate to its world position
        /// and its destination
        /// </summary>
        public void Update()
        {

            Vector3 direction = new Vector3(destination.Length() / 2, 0, 0);
            
            cuboid.Scale(new Vector3(destination.Length() / 2, 5f, 5f));
            cuboid.Translate(direction);

            cuboid.Rotate(SphereCoordinateOrientation(destination));
            cuboid.Translate(worldCoordinate);

            cuboid.Update();
        }

        /// <summary>
        /// Draws the bone's appearance
        /// </summary>
        /// <param name="device">the device to draw the bone's appearance</param>
        public void Draw(GraphicsDevice device)
        {
            cuboid.Draw(device);
        }
   
        /// <summary>
        /// generates the bone's rotation by unsing sphere coordinates
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private Vector3 SphereCoordinateOrientation(Vector3 position)
        {
            float alpha = 0;
            float beta = 0;
            if (position.Z != 0.0 || position.X != 0.0)
                alpha = (float)Math.Atan2(position.Z, position.X);

            if (position.Y != 0.0)
                beta = (float)Math.Atan2(position.Y, Math.Sqrt(position.X * position.X + position.Z * position.Z));

            return new Vector3(0, -alpha, beta);
        }


        /// <summary>
        /// the bone's destination is local and points to the world's destination
        /// so this function just subtract's the bone's world coordinate from the world's destination
        /// and gets the bone's local destination vector
        /// </summary>
        /// <param name="destination">The destination in the world coordinate system</param>
        public void SetLocalDestinationbyAWorldDestination(Vector3 destination)
        {
            this.destination = destination - worldCoordinate;
        }

        /// <summary>
        /// the bone's source plus the bone's destination vector
        /// </summary>
        /// <returns></returns>
        public Vector3 Effector
        {
            get 
            {
                return worldCoordinate + destination;
            }
        }
    }
}
